#include "Includes.h"
///////////////////////////////////////////////////////////////////////////////////////
int src_deg_Blue= 0;
int dst_deg_Blue= 0;

int src_deg_Red= 0;
int dst_deg_Red= 0;

#define delta_src_dst 430

int src_deg=0;
int dst_deg=0;


/////////////////////////////////////////////
void Turning_Arm_Func(int speed)
{
	motor[Mtr_Arm]=speed;
}

void Turning_Arm_Handle()
{
	if(      vexRT[Btn8L]  && !vexRT[Btn8R]  )
		Turning_Arm_Func(-45);
	else if( vexRT[Btn8R]  && !vexRT[Btn8L]  )
		Turning_Arm_Func(45);
	else
		Turning_Arm_Func(0);
	while (Special_Key)
	{
		if( vexRT[Btn8L]  && !vexRT[Btn8R]  )
			Turning_Arm_Func(-25);
		else if( vexRT[Btn8R]  && !vexRT[Btn8L]  )
			Turning_Arm_Func(25);
		else if( ! vexRT[Btn8R] && ! vexRT[Btn8L] )
		{
			Turning_Arm_Func(0);
			break;
		}
	}
}
///////////////////////////////////////////////////////////////////////////////////////
//Turn Arm
int GBL_Turning_Arm_AngleDeg=0;
int GBL_Turning_Arm_Max_Speed=0;
int GBL_Turning_Arm_Exit_Time=0;
TTimers GBL_Turning_Arm_Timer_x=0;

int GBL_Turning_Arm_exit_ahead=0;
int GBL_Turning_Arm_dir=1;

task Turning_Arm_Encoder_task()
{
	float Kp =1.5,Ki =0.00025,Kd =0.005;
	float Output =0;
	int Last_Err =0,Err =0;
	float Integral =0,Derivation =0;
	//y

	int Aim_Ticks= GBL_Turning_Arm_AngleDeg;

	int temp=nMotorEncoder[Mtr_Arm];

	/*******************************************************************************
	sem_Turning_Arm
	*************************************************************************************/
	while(1)
	{
		SemaphoreLock(sem_Turning_Arm,50);
		if(bDoesTaskOwnSemaphore(sem_Turning_Arm) )
			break;
	}
	/*******************************************************************************
	*************************************************************************************/
	if(temp>= GBL_Turning_Arm_AngleDeg)
		GBL_Turning_Arm_dir=-1;
	else
		GBL_Turning_Arm_dir=1;

	ClearTimer(GBL_Turning_Arm_Timer_x);
	while(time1[GBL_Turning_Arm_Timer_x]<GBL_Turning_Arm_Exit_Time)
	{
		//x
		temp = nMotorEncoder[Mtr_Arm];
		Err =Aim_Ticks - temp;
		if(abs(Err )<10) //blind area
			Err =0;
		if(abs(Err )<40)
			Integral +=Err ;
		Derivation =Err -Last_Err ;
		Output =Kp *Err +Ki *Integral +Kd *Derivation ; //calc output
		if(abs(Output)>GBL_Turning_Arm_Max_Speed)
			Output=sgn(Output)*GBL_Turning_Arm_Max_Speed;
		Last_Err =Err ;
		//output all
		motor[Mtr_Arm]=Output;
		wait1Msec(2);
		/*******************************************************************************

		*************************************************************************************/
		if(GBL_Turning_Arm_exit_ahead==1)
		{
			if(FIELD==Blue_Field)
			{
				if( GBL_Turning_Arm_dir ==1 && (Err==0 || temp>=Aim_Ticks ) )
				{
					writeDebugStream("exit");
					break;
				}
			}
			if(FIELD==Red_Field)
			{
				if( GBL_Turning_Arm_dir ==-1 && (Err==0 || temp<=Aim_Ticks ) )
				{
					writeDebugStream("exit");
					break;
				}
			}
		}
	}
	/*******************************************************************************

	*************************************************************************************/
	motor[Mtr_Arm] = 0;
	writeDebugStream("turn: %d,%d,%d\n",time1[GBL_Turning_Arm_Timer_x],Aim_Ticks, temp);
	/*******************************************************************************
	*************************************************************************************/
	SemaphoreUnlock(sem_Turning_Arm);////////////////////////
}
//////////////////////
void Turning_Arm_Encoder_TASK_FUN(int AngleDeg,int Max_Speed,int Exit_Time,TTimers Timer_x,int exit_ahead)
{
	GBL_Turning_Arm_AngleDeg=AngleDeg;
	GBL_Turning_Arm_Max_Speed=Max_Speed;
	GBL_Turning_Arm_Exit_Time=Exit_Time;
	GBL_Turning_Arm_Timer_x=Timer_x;
	StartTask(Turning_Arm_Encoder_task);
}
